#include <ros/ros.h>
#include <tinyxml.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nav_msgs/Odometry.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/PoseStamped.h"
#include <geometry_msgs/Pose.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include "std_msgs/String.h"

using namespace std;
// using namespace tf;

float x_current;
float y_current;

float normeNextGoal;

class Waypoint_goal
{
public:
  struct Point
  {
    string name;
    float x;
    float y;

    float z;
    float w;
    bool visited;
  };

  vector<Point> Points;

  //Path_planned(float x, float y, bool visited);
  void addpointGoal(string Name, float X, float Y, float Z, float W, bool visit);
};



void Waypoint_goal::addpointGoal(string Name, float X, float Y, float Z, float W, bool visit) {
    Waypoint_goal::Point newPoint;
    newPoint.name = Name;
    newPoint.x = X;
    newPoint.y = Y;
    newPoint.z = Z;
    newPoint.w = W;
    newPoint.visited = visit;
    Points.push_back(newPoint);
}

Waypoint_goal newWaypoint;

nav_msgs::Path passed_path;
ros::Publisher pub_passed_path;

void pose_callback(const geometry_msgs::Pose &poses)
{ //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点
  x_current = poses.position.x;
  y_current = poses.position.y;
  passed_path.header.frame_id ="map";
  passed_path.header.stamp = ros::Time::now();
  geometry_msgs::PoseStamped p;
  p.header = passed_path.header;
  p.pose = poses;
  passed_path.poses.emplace_back(p);
  pub_passed_path.publish(passed_path);
}

// 加载导航点数据
bool LoadWaypointsFromFile(string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {
        ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str());
        return false;
    }

    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load waypoint : %s", name);

        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();

        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();

        newWaypoint.addpointGoal(  string(name), atof(pos_x),  atof(pos_y),  atof(ori_z),  atof(ori_w), false);
        cout <<"debug cwx:"<< string(name)  <<string(pos_x)  <<string(pos_y)  <<string(pos_z)  <<string(ori_z)  <<string(ori_w)  <<endl;
    }
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "waypoint_goal");
    ros::NodeHandle waypoint_goal;
    string waypoint_path; 

    bool way_finished = false;

    waypoint_goal.param<string>("waypointXML",  waypoint_path, "/home/wheeltec-client/cwx_worksp/qt_rviz_hub2/src/Ros_Qt_Rviz_GUI-main/exist_waypoint/test_waypoint.xml");
    
    ROS_INFO("Load waypoint_path : %s", waypoint_path.c_str());

    LoadWaypointsFromFile(waypoint_path);
    
    ros::Subscriber sub1 = waypoint_goal.subscribe("/robot_pose_me", 10, pose_callback);

    ros::Publisher pub1 = waypoint_goal.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    pub_passed_path = waypoint_goal.advertise<nav_msgs::Path>("/clean_robot/passed_path", 10);
   
    ros::Publisher finish_pub =  waypoint_goal.advertise<std_msgs::String>("/clean_robot/finish", 1000);

    ros::Rate loop_rate(10);

    geometry_msgs::PoseStamped goal_msgs;
    int count = 0;

    bool goal_reached = false;

    ros::Duration(2.0).sleep();

    // //获取发送下一个点的阈值
    // if (!next_goal.getParam("/NextGoal/tolerance_goal", normeNextGoal))
    // {
    //     ROS_ERROR("Please set your tolerance_goal");
    //     return 0;
    // }
    // ROS_INFO("tolerance_goal=%f", normeNextGoal);

    while (ros::ok())
    {
        ros::spinOnce();
        // ROS_INFO("cwx_debug:while");
        // //当前处理的点
        // cout << " points size : " << newWaypoint.Points.size() << endl;
        if (!newWaypoint.Points.empty())
        {
            //当前距离达到了
            if (sqrt(pow(x_current - newWaypoint.Points[count].x, 2) + pow(y_current - newWaypoint.Points[count].y, 2)) <= 0.5)
            {
                // count++;
                // goal_reached = false;
                if (count < newWaypoint.Points.size() - 1) {
                   count++;
                   goal_reached = false;
                } else if (count == newWaypoint.Points.size() - 1) {
                   way_finished = true;
                   std_msgs::String output_msg;
                   output_msg.data = "Route planning is complete";
                   finish_pub.publish(output_msg);
                }
            }
            if (goal_reached == false)
            {
                goal_msgs.header.frame_id = "map";
                goal_msgs.header.stamp = ros::Time::now();
                goal_msgs.pose.position.x = newWaypoint.Points[count].x;
                goal_msgs.pose.position.y = newWaypoint.Points[count].y;
                goal_msgs.pose.position.z = 0;
                goal_msgs.pose.orientation.w = newWaypoint.Points[count].w;
                goal_msgs.pose.orientation.x = 0;
                goal_msgs.pose.orientation.y = 0;
                goal_msgs.pose.orientation.z = newWaypoint.Points[count].z;

                cout << " NEW GOAL count:"<< count << "  x = " << newWaypoint.Points[count].x << "  y = " << newWaypoint.Points[count].y << endl;
                goal_reached = true;

                pub1.publish(goal_msgs);
            }
            cout << x_current << "  " << y_current << endl;
            // //当前
            cout << newWaypoint.Points[count].x << "  " << newWaypoint.Points[count].y << endl;
            // //目标
            cout << " DISTANCE : " << sqrt((x_current - newWaypoint.Points[count].x) * (x_current - newWaypoint.Points[count].x) + (y_current - newWaypoint.Points[count].y) * (y_current - newWaypoint.Points[count].y)) << endl;
            // // 距离公式
         }

        if (way_finished) {
            break;
        }
        loop_rate.sleep();
    }

    return 0;
}
